#ifndef THETA_PID_SOURCE_H
#define THETA_PID_SOURCE_H

#include "Include.h"

namespace Team675
{

class ThetaPIDSource : public PIDSource {
public:
	ThetaPIDSource(Gyro& _gyro, Encoder& _encoder, float _startAngle) : 
		m_gyro(_gyro), 
		m_encoder(_encoder) 
	{
		m_encoderOffset = _startAngle;
		m_setPoint = _startAngle;
	}
	
	double PIDGet() {
		return m_gyro.PIDGet() + GetEncoderValue() - m_setPoint;
	}
	
	void SetEncoderValue(float _encoderValue) {
		m_encoderOffset = _encoderValue - m_encoder.GetDistance();
	}
	
	float GetEncoderValue() {
		return m_encoder.PIDGet() + m_encoderOffset;
	}
	
	void changeSetPoint(float change) {
		m_setPoint += change;
		float encoderValueGoal = m_setPoint - m_gyro.PIDGet(); //encoder value that would make PIDGet() return 0
		if(1) {
			if(encoderValueGoal < 0) {
				m_setPoint -= encoderValueGoal;
			}
			else if(encoderValueGoal > 360) {
				m_setPoint -= 360 - encoderValueGoal;
			}
		}
	}
private:
	Gyro &m_gyro; //gyro that measures the rotation of the robot
	Encoder &m_encoder; //encoder that measures the rotation of the turret
	float m_encoderOffset; //difference between the encoder reading and the actual turret angle(degrees)
	float m_setPoint; //the angle that you are turning the turret to with respect to the field
	bool m_stop; //if true: stop at the edges. if false: turn back around at the edges
};

}

#endif
